package controlSoftware.DataProcess;

import java.util.Calendar;

/**
 * Data objects that includes all necessary values for data manipulation
 */
public class SensorData{
	private long timeStamp;
	
	/** IMU measurement **/
	private double accel_x;
	public double getAccel_x() {
		return this.accel_x;
	}
	private double accel_y;
	public double getAccel_y() {
		return this.accel_y;
	}
	
	private double accel_z;
	public double getAccel_z() {
		return this.accel_z;
	}
	
	private double pitch;
	private double roll;
	private double yaw;
	
	// Calculated velocities
	private Velocity currVel;
	public Velocity getCurrVel() {
		return this.currVel;
	}

	// Calculated displacements
	private double disp_x;
	private double disp_y;
	
	// Coordinates for rendering
	public Coordinate currLoc;
	public Coordinate getCurrLoc() {
		return this.currLoc;
	}
	
	
	/**
	 * Creates Sensor data object
	 * @param time stamp
	 * @param x
	 * @param y
	 * @param z
	 * @param pitch
	 * @param roll
	 * @param yaw
	 */
	public SensorData(long timeStamp, double x, double y, double z, double pitch, double roll, double yaw){
		this.timeStamp = timeStamp;
		this.accel_x = x;
		this.accel_y = y;
		this.accel_z = z;
		this.pitch = pitch;
		this.roll = roll;
		this.yaw = yaw;
		
		currVel = new Velocity(0,0);
		currLoc = new Coordinate (0,0);
	}
	
	/**
	 * Applies first level filtering to IMU readings
	 * @param acceleration value of the previous data package
	 * @return
	 */
	public int processRaw(double old_x, double old_y)
	{
		// converts IMU readings to acceleration values
		//System.out.println("processRaw raw current => " + accel_x + " , " + accel_y);
		//System.out.println("processRaw raw old => " + old_x + " , " + old_y);
		
		accel_x = (accel_x - SysModel.ax_offset);
		accel_y = (accel_y - SysModel.ay_offset);
		
		// if the change in acceleration from previous data is small, 
		// then the new data is disregarded
		if (Math.abs(accel_x - old_x)<1.5)
		{
			if (Math.abs(accel_x) < 1) // round acceleration to 0 if its very small
			{
				accel_x = 0;
			}
			else 
			{
				accel_x = old_x;
			}
		}else {
			if (Math.abs(accel_x) < 1)
			{
				accel_x = 0;
			}
		}
		
		if (Math.abs(Math.abs(accel_y) - Math.abs(old_y))<1.5)
		{
			if (Math.abs(accel_y) < 1)
			{
				accel_y = 0;
			}
			else
			{
				accel_y = old_y;
			}
		}else {
			if (Math.abs(accel_y) < 1)
			{
				accel_y = 0;
			}
		}
		
		accel_x = (accel_x * 9.80665d) / SysModel.gravity;
		accel_y = (accel_y * 9.80665d) / SysModel.gravity;
		System.out.println("processRaw -> "+accel_x + " , " + accel_y);
		return 0; 
	}
	
	/**
	 * Calculate velocity change between current and previous data object
	 * @param previous velocity
	 * @param time difference
	 * @return current absolute velocity
	 */
	public Velocity getVelocity(Velocity prevV, long sampleTime)
	{
		if (currVel!= null){
			currVel.setVel_x(currVel.calcNewVelocity(prevV.getVel_x(), accel_x, sampleTime));
			currVel.setVel_y(currVel.calcNewVelocity(prevV.getVel_y(), accel_y, sampleTime));
		}		
		System.out.println("velocity -> " +currVel.getVel_x() + " , " + currVel.getVel_y());
		return currVel;
	}

	/**
	 * Calculates the change in displacement from velocity and time
	 * @param prevVel
	 * @param sampleTime
	 * @return
	 */
	public int getDisplacement(Velocity prevVel, long sampleTime) 
	{
		
		//disp_x = (prevVel.getVel_x() + currVel.getVel_x())/2 * sampleTime/1000;
		//disp_y = (prevVel.getVel_y() + currVel.getVel_y())/2 * sampleTime/1000;
		
		// formulat: d = Vo*t + 0.5att
		disp_x = (prevVel.getVel_x()*SysModel.sampleTime/1000) + (0.5*accel_x*SysModel.sampleTime/1000*SysModel.sampleTime/1000);
		disp_y = (prevVel.getVel_y()*SysModel.sampleTime/1000) + (0.5*accel_y*SysModel.sampleTime/1000*SysModel.sampleTime/1000);
		System.out.println("Disp -> " + disp_x + " , " + disp_y);
		return 1;	
	}
	
	/**
	 * Calculates the new coordinate from previous coordinate and displacement
	 * @param prev
	 * @return
	 */
	public Coordinate getCoordinate(Coordinate prev)
	{
		// disp [m] * ratio [pix/m] = x [pix]
		currLoc.setX(prev.getX() + (disp_x * SysModel.compression_ratio));
		currLoc.setY(prev.getY() + (disp_y * SysModel.compression_ratio));
		System.out.println("coord -> " + currLoc.getX() + " , " + currLoc.getY());
		return currLoc;
	}
	
	public long getTimeStamp() {
		return timeStamp;
	}

	@Override
	public String toString(){
		StringBuilder builder = new StringBuilder();
		builder.append("TIME STAMP: " + this.timeStamp);
		builder.append("x: " + this.accel_x);
		builder.append("y: "+this.accel_y);
		builder.append("z: " + this.accel_z);
		builder.append("pitch: " + this.pitch);
		builder.append("roll: " + this.roll);
		builder.append("yaw: " + this.yaw);
		 
		builder.append("disp_x: "+ this.disp_x);
		builder.append("disp_y: " + this.disp_y);
		builder.append("velocity_x: " + this.currVel.getVel_x());
		builder.append("velocity_y: " + this.currVel.getVel_y());
		
		String toString = builder.toString();
		return toString;	
	}
}